#include "radar.h"
#include <math.h>
#include <stdio.h>
#include "rplidar.h"

using namespace rp::standalone::rplidar;


RPlidarDriver* drv = NULL;
rplidar_response_measurement_node_t nodes[1024];

int radar_init(const char* port_name)
{
	u_result op_result;
	rplidar_response_device_info_t devinfo;

	drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);

	if (!drv)
	{
		printf("insufficent memory, exit\n");
		return -1;
	}

	if(IS_OK(drv->connect(port_name, 115200)))
	{
		op_result = drv->getDeviceInfo(devinfo);

		if (!IS_OK(op_result))
		{
			delete drv;
			drv = NULL;
			return -1;
		}

		drv->startMotor();
		drv->startScan(0, 1);

		printf("open radar successfully\n");

		return 0;
	}

	return -1;
}


void radar_close(void)
{
	drv->stop();
	drv->stopMotor();

	RPlidarDriver::DisposeDriver(drv);
	drv = NULL;
}


double get_radar_info(void)
{
	size_t   count = sizeof(nodes)/sizeof(nodes[0]);
	u_result op_result= drv->grabScanData(nodes, count);

	double err = -2;
	double angle = 0;
	double distance = 6 * SAFETY_DISTANCE;

	if (IS_OK(op_result))
	{
		drv->ascendScanData(nodes, count);

		for (int pos = 0; pos < (int)count ; ++pos)
		{
			if((nodes[pos].sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) > SIGNAL_QUALITY)
			{
				nodes[pos].angle_q6_checkbit = ((nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f);

				if(nodes[pos].angle_q6_checkbit < SCAN_AREA || nodes[pos].angle_q6_checkbit > 360 - SCAN_AREA)
				{
					nodes[pos].distance_q2 = (nodes[pos].distance_q2 / 4.0f);

					if(distance > nodes[pos].distance_q2)
					{
						distance = nodes[pos].distance_q2;
						angle = nodes[pos].angle_q6_checkbit;
					}
				}
			}
		}

		angle = angle > 180 ? 360 - angle : -1 * angle;
		err = distance < SAFETY_DISTANCE ? pow(1 - (distance * fabs(angle)) / (SAFETY_DISTANCE * SCAN_AREA), 2) : 0;
		err = angle >=0 ? err : -1 * err;
		err = distance < DANGEROUS_DISTANCE ? (angle >= 0 ? -1 : 1) : err;
	}

	return err;
}


int test_radar(void)
{
	radar_init("/dev/ttyUSB0");
	double err;

	for(int i = 0; i < 1000; i++)
	{
		err = get_radar_info();
		printf("%f\n", err);
	}

	radar_close();
	return 0;
}

